/*
 * Copyright 2023 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : imu_project.h
 *@brief   : imu project
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/10/07
 *
 */

#ifndef IMU_PROJECT_H_
#define IMU_PROJECT_H_	

#include <string>     // 添加string头文件
#include <algorithm>  // 添加algorithm头文件
#include <array>
#include <queue>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <iostream>

using namespace std;

class ImuProject{
public:
    ImuProject(double g = 9.81) : g_(g) {
        // 初始化为单位矩阵，后续会根据姿态更新
        R_B2B_ = Eigen::Matrix3d::Identity();
        z_B2_ = Eigen::Vector3d::UnitZ();
    }

    // 更新平面坐标系（基于当前姿态）
    void updatePlaneFrame(const Eigen::Matrix3d& R_BW) {
        // 1. 计算平面法线方向（世界系Z轴在{B}系中的方向）
        Eigen::Vector3d z_W = Eigen::Vector3d::UnitZ();  // 世界系Z轴
        z_B2_ = R_BW * z_W;  // 转换到{B}系
        z_B2_.normalize();   // 单位化

        // std::cout << "z_W ========= : " << z_W.transpose() << std::endl;
        // std::cout << "z_B2_ ========= : " << z_B2_.transpose() << std::endl;

        // 2. 构建{B₂}系的X轴（将{B}系X轴投影到平面上）
        Eigen::Vector3d x_temp = Eigen::Vector3d::UnitX();  // {B}系X轴
        Eigen::Vector3d x_B2 = x_temp - (x_temp.dot(z_B2_)) * z_B2_;
        x_B2.normalize();
        
        // 3. 构建{B₂}系的Y轴（通过叉积）
        Eigen::Vector3d y_B2 = z_B2_.cross(x_B2);
        y_B2.normalize();
        
        // 4. 构建旋转矩阵 R_B2B
        R_B2B_.col(0) = x_B2;
        R_B2B_.col(1) = y_B2;
        R_B2B_.col(2) = z_B2_;
    }
    
    // 处理IMU数据：去重力 + 投影到2D
    void processIMUData(const Eigen::Vector3d& a_meas_B, 
                       const Eigen::Vector3d& omega_B,
                       const Eigen::Matrix3d& R_BW,
                       Eigen::Vector2d& a_2D,
                       double& omega_yaw) {
        
        // 更新平面坐标系
        updatePlaneFrame(R_BW);
        
        // 1. 去除重力影响
        Eigen::Vector3d g_W(0.0, 0.0, g_);  // 世界系中的重力向量
        Eigen::Vector3d g_B = R_BW * g_W;     // {B}系中的重力分量
        Eigen::Vector3d a_motion_B = a_meas_B - g_B;
        
        // std::cout << "g_B ========= : " << g_B.transpose() << std::endl;

        // std::cout << "a_motion_B ========= : " << a_motion_B.transpose() << std::endl;

        // 2. 投影到{B₂}系
        Eigen::Vector3d a_motion_B2 = R_B2B_ * a_motion_B;
        
        // 3. 提取二维加速度（取X-Y分量）
        a_2D = a_motion_B2.head<2>();
        
        // 4. 计算偏航角速度（绕平面法线的角速度分量）
        omega_yaw = omega_B.dot(z_B2_);
    }
    
    // 获取当前平面法线方向（用于调试）
    Eigen::Vector3d getPlaneNormal() const {
        return z_B2_;
    }
    
    // 获取当前旋转矩阵（用于调试）
    Eigen::Matrix3d getRotationMatrix() const {
        return R_B2B_;
    }
private:
    double g_;  // 重力加速度
    Eigen::Vector3d z_B2_;  // 平面法线在{B}系中的方向
    Eigen::Matrix3d R_B2B_;  // 从{B}到{B₂}的旋转矩阵

};

#endif